/*
 *	fastresample.c
 *
 *	Use linear interpolation to resample the sound data frequence.
 *	Fast algorithm with straight Int/Short operation on linear interpolation.
 *	Quality is not as good as advanced algorithm with quadratic interpolation
 *      and anti-frequence shift. However, this shall be good enough for our
 *	application which sound quality is not so critical.
 *
 *	Copy from Standford Sound Research Lab web site. W/o original copy
 *      right owner statements on it. But its free for use for sure.
 */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>

#include "fastresample.h"
#define __DEBUG

#include "Trace.h"

#define IBUFFSIZE 4096                         /* Input buffer size */
//#define OBUFFSIZE (IBUFFSIZE*MAXFACTOR+2)      /* Calc'd out buffer size */

static inline HWORD WordToHword(LWORD v, int scl)
{
	HWORD out;
	LWORD llsb = (1<<(scl-1));
	v += llsb;		/* round */
	v >>= scl;
	if (v>MAX_HWORD)
		v = MAX_HWORD;
	else if (v < MIN_HWORD)
		v = MIN_HWORD;
	out = (HWORD) v;
	return out;
}

/* Sampling rate conversion using linear interpolation for maximum speed.
 */
static int  SrcLinear(HWORD X[], HWORD Y[], double factor, ULWORD *Time, UHWORD Nx)
{
	HWORD iconst;
	HWORD *Xp, *Ystart;
	LWORD v,x1,x2;

	double dt;                  /* Step through input signal */
	ULWORD dtb;                 /* Fixed-point version of Dt */
	ULWORD endTime;             /* When Time reaches EndTime, return to user */

	dt = 1.0/factor;            /* Output sampling period */
	dtb = dt*(1<<Np) + 0.5;     /* Fixed-point representation */

	Ystart = Y;
	endTime = *Time + (1<<Np)*(LWORD)Nx;
	while (*Time < endTime)
	{
		iconst = (*Time) & Pmask;
		Xp = &X[(*Time)>>Np];      /* Ptr to current input sample */
		x1 = *Xp++;
		x2 = *Xp;
		x1 *= ((1<<Np)-iconst);
		x2 *= iconst;
		v = x1 + x2;
		*Y++ = WordToHword(v,Np);   /* Deposit output */
		*Time += dtb;		    /* Move to next sample by time increment */
	}
	return (Y - Ystart);            /* Return number of output samples */
}

static int readData(HWORD *inArray,
		    HWORD **inPtr,
		    int inCount,
		    HWORD *outPtr1,
		    HWORD *outPtr2,
		    int dataArraySize,
		    int nChans, int Xoff)
    /* return: 0 - notDone */
    /*        >0 - index of last sample */
{
	int Nsamps, val=0;
	HWORD *dataStart;
	HWORD *inend = inArray + inCount*nChans;

	dataStart = outPtr1;
	Nsamps = dataArraySize - Xoff; /* Calculate number of samples to get */
	outPtr1 += Xoff;		/* Start at designated sample number */
	if (nChans==1)
	{
		for (; Nsamps>0; Nsamps--)
		{
			if (*inPtr==inend) break;
			*outPtr1++ = *(*inPtr)++;
		}
	} else {
		outPtr2 += Xoff;		/* Start at designated sample number */
		for (; Nsamps>0; Nsamps--)
		{
			if (*inPtr==inend) break;
			*outPtr1++ = *(*inPtr)++;
			*outPtr2++ = *(*inPtr)++;
		}
	}
	if (Nsamps > 0)
	{
		val = outPtr1 - dataStart; 	/* (Calc return value) */
		while (--Nsamps >= 0)
		{				/*   fill unread spaces with 0's */
			*outPtr1++ = 0;		/*   and return FALSE */
			if (nChans==2) *outPtr2++ = 0;
		}
	}
	return(val);
}

int resampleFast(  /* number of output samples returned */
    double factor,		/* factor = Sndout/Sndin */
    HWORD *inPtr,		/* input data pointer (data size shall be inCount * nChans) */
    HWORD *outPtr,		/* output data pointer (data size shall be outCount * nChans) */
    int inCount,		/* number of input samples to convert */
    int outCount,		/* number of output samples to compute */
    int nChans)			/* number of sound channels (1 or 2) */
{
	ULWORD Time, Time2;		/* Current time/pos in input sample */
	UHWORD Xp, Ncreep, Xoff, Xread;
	int OBUFFSIZE = (int)(((double)IBUFFSIZE)*factor+2.0);
	HWORD X1[IBUFFSIZE], Y1[OBUFFSIZE]; /* I/O buffers */
	HWORD X2[IBUFFSIZE], Y2[OBUFFSIZE]; /* I/O buffers */
	UHWORD Nout, Nx;
	HWORD *inPtrRun = inPtr;	/* Running pointer thru input */
	int i, Ycount, last;

	Xoff = 10;

	Nx = IBUFFSIZE - 2*Xoff;     	/* # of samples to process each iteration */
	last = 0;			/* Have not read last input sample yet */
	Ycount = 0;			/* Current sample and length of output file */

	Xp = Xoff;			/* Current "now"-sample pointer for input */
	Xread = Xoff;			/* Position in input array to read into */
	Time = (Xoff<<Np);		/* Current-time pointer for converter */

	for (i=0; i<Xoff; X1[i++]=0); 	/* Need Xoff zeros at begining of sample */
	for (i=0; i<Xoff; X2[i++]=0); 	/* Need Xoff zeros at begining of sample */

	do {
		if (!last)		/* If haven't read last sample yet */
		{
			last = readData(inPtr, &inPtrRun, inCount, X1, X2,
					IBUFFSIZE, nChans, (int)Xread);
			if (last && (last-Xoff<Nx))
			{ 				/* If last sample has been read... */
				Nx = last-Xoff;		/* ...calc last sample affected by filter */
				if (Nx <= 0) break;
			}
		}

		/* Resample stuff in input buffer */
		Time2 = Time;
		Nout=SrcLinear(X1,Y1,factor,&Time,Nx);
		if (nChans==2)
			Nout = SrcLinear(X2,Y2,factor,&Time2,Nx);

		Time -= (Nx<<Np);	/* Move converter Nx samples back in time */
		Xp += Nx;		/* Advance by number of samples processed */
		Ncreep = (Time>>Np) - Xoff; /* Calc time accumulation in Time */
		if (Ncreep)
		{
			Time -= (Ncreep<<Np);    /* Remove time accumulation */
			Xp += Ncreep;            /* and add it to read pointer */
		}
		for (i=0; i<IBUFFSIZE-Xp+Xoff; i++)
		{ 				/* Copy part of input signal */
			X1[i] = X1[i+Xp-Xoff]; /* that must be re-used */
			if (nChans==2)
				X2[i] = X2[i+Xp-Xoff]; /* that must be re-used */
		}
		if (last)
		{				/* If near end of sample... */
			last -= Xp;		/* ...keep track were it ends */
			if (!last)		/* Lengthen input by 1 sample if... */
				last++;		/* ...needed to keep flag TRUE */
		}
		Xread = i;			/* Pos in input buff to read new data into */
		Xp = Xoff;

		Ycount += Nout;
		if (Ycount>outCount)
		{
			Nout -= (Ycount-outCount);
			Ycount = outCount;
		}

		if (Nout > OBUFFSIZE) /* Check to see if output buff overflowed */
		{
			TRACE("Output array overflow!\n");
			return -1;
		}

		if (nChans==1)
		{
			register HWORD *y1p = &(Y1[0]);
			register HWORD *op = outPtr;
			while (Nout--)		/* Nout is UNSIGNED */
				*op++ = *y1p++;
			outPtr = op;
		} else {
			register HWORD *y1p = &(Y1[0]);
			register HWORD *y2p = &(Y2[0]);
			register HWORD *op = outPtr;
			while (Nout--)
			{
				*op++ = *y1p++;
				*op++ = *y2p++;
			}
			outPtr = op;
		}
	} while (Ycount<outCount); 	/* Continue until done */
	return(Ycount);			/* Return # of samples in output file */
}

// resampleContext

resampleContext *resampleContext_init( int in_channels, int out_channels, int in_rate, int out_rate )
{
	resampleContext *context = NULL;

	/* is reample required ? */
	if ( in_channels == out_channels && in_rate == out_rate )
		return NULL;

	context = malloc( sizeof(resampleContext) );
	memset( context, 0, sizeof(resampleContext) );
	if ( context != NULL )
	{
		context->in_channels 	= in_channels;
		context->out_channels 	= out_channels;
		context->in_sample 	= in_rate;
		context->out_sample	= out_rate;
		context->factor = (double)out_rate / in_rate;
	}
	return context;
}

void resampleContext_destroy( resampleContext *context )
{
	if ( context->mid_buffer )
		free( context->mid_buffer );
	free( context );
}

/* fill_channel:
 * fill sound data from client channel data to sound output buffer (stereo)
 * 	mono -> left + right
 * 	mono -> left
 * 	mono -> right
 */
static void fill_channel( HWORD *src, int nsample, int channels, HWORD *stereo )
{
	int	i;
	HWORD	*dst = stereo;
	register HWORD	sVal;
	
	if ( channels == 1 )		// fill left + right
	{
		for(i=0; i<nsample; i++, src++)
		{
			sVal = *src / 2;
			*dst++ = sVal;
			*dst++ = sVal;
		}
	}
	else			// fill to left or right
	{
		if ( channels == -2 )
			*dst++ = 0;		// point to first sample of right channel
		for(i=0; i<nsample; i++, src++)
		{
			*dst++ = *src;
			*dst++ = 0;		// zero channel we don't want
		}
	}
}

// pick mono, left or right out of stereo
// nsample: number of samples ( # of short )
static void pick_channel( HWORD *stereo, int nsample, int channels, HWORD *mono )
{
	int	i, val;

	if ( channels == 1 )		// pick left + right -> mono
	{
		for(i=0; i<nsample/2; i++)
		{
			val = (*stereo + *(stereo+1));
			if ( val > MAX_HWORD ) val = MAX_HWORD;
			else if ( val < MIN_HWORD ) val = MIN_HWORD;
			*mono++ = val;
			stereo += 2;
		}
	}
	else				// pick left or right
	{
		if ( channels == -2 )
			stereo++;	// point to first sample we want
		for(i=0; i<nsample/2; i++)
		{
			*mono++ = *stereo++;
			stereo++;
		}
	}
}

// resample and mixing

void SND_resample( resampleContext *context, HWORD *iBuf, int inSize, HWORD *oBuf, int outSize )
{
	/* check for middle buffer - allocate when required */
	if ( context->in_channels != context->out_channels &&
	     context->in_sample != context->out_sample )
	{
		int	mid_size = (context->in_channels == 2) ? (inSize >> 1) : (inSize << 1);
		if (context->mid_buffer==NULL || context->mid_bufsize != mid_size )
		{
			/* we need a middle buffer */
			if ( context->mid_buffer )	free( context->mid_buffer );
			context->mid_buffer = malloc( mid_size * sizeof(short) );
			context->mid_bufsize = mid_size;
		}
	}

	/* if in/out channels are different, make them alike first */
	if ( context->in_channels != context->out_channels )
	{
		if ( context->in_channels == 2 )
		{
			if ( context->in_sample == context->out_sample )
				pick_channel( iBuf, inSize, context->out_channels, oBuf );
			else
				pick_channel( iBuf, inSize, context->out_channels, context->mid_buffer );
		}
		else if ( context->out_channels == 2 )
		{
			if ( context->in_sample == context->out_sample )
				fill_channel( iBuf, inSize, context->in_channels, oBuf );
			else
				fill_channel( iBuf, inSize, context->in_channels, context->mid_buffer );
		}
		else 	// both ends are single channel. Just copy
		{
			if ( context->in_sample == context->out_sample )
				memcpy( oBuf, iBuf, inSize*sizeof(short) );
			else
				memcpy( context->mid_buffer, iBuf, inSize*sizeof(short) );
		}
	}
	/* now we can reample the input */
	if ( context->in_sample != context->out_sample )
	{
		int	out_channels = context->out_channels==2 ? 2 : 1;
		if ( context->in_channels != context->out_channels )
			resampleFast( context->factor, context->mid_buffer, oBuf,
				      context->mid_bufsize/out_channels, outSize/out_channels, out_channels );
		else
			resampleFast( context->factor, iBuf, oBuf,
				      inSize/out_channels, outSize/out_channels, out_channels );
	}
}

void SND_mixing( HWORD *audio, HWORD *audio_sum, int nsample )
{
	int	val;
	int	i;

	for( i=0; i<nsample; i++)
	{
		val = *audio_sum;
		val += *audio++;
		if ( val > MAX_HWORD )		val = MAX_HWORD;
		else if ( val < MIN_HWORD )	val = MIN_HWORD;
		*audio_sum++ = val;
	}
}
